%% 投影到可行域
% u = [t1x t1y t2x t2y t3x t3y ... tNx tNy rx ry]
function u = enforce_constraints_origin(u, d_min, Ct_bounds, Cr_bounds)
    % 约束投影，确保粒子满足最小距离约束，并在可行范围内
    % u: 粒子位置向量 [t1x, t1y, t2x, t2y, ..., rx, ry]
    % d_min: 最小距离
    % Ct_bounds: 发射天线的可行范围，形如 [x_min, x_max; y_min, y_max]
    % Cr_bounds: 接收天线的可行范围，形如 [rx_min, rx_max; ry_min, ry_max]

    % 提取天线信息
    N = (length(u) - 2) / 2; % 发射天线个数
    t = reshape(u(1:2*N), [2, N]); % 发射天线坐标 [2 × N]
    r = u(2*N+1:end);              % 接收天线坐标 [2 × 1]
    eps = 0.000001;

    % --- 最小距离约束 ---
    for i = 1:N
        for j = (i+1):N
            % 计算两天线的欧几里得距离
            dist = norm(t(:, i) - t(:, j));
            if (  (t(2, i) == Ct_bounds(2,1)) && (t(2, j) == Ct_bounds(2,1)) )
                t(2, j) = Ct_bounds(2,1) +  d_min;
                continue
            end
            if (  (t(2, i) == Ct_bounds(2,2)) && (t(2, j) == Ct_bounds(2,2)) )
                t(2, i) = Ct_bounds(2,2) - d_min;
                continue
            end
            if dist < d_min
                % 调整 j 天线的位置
                direction = (t(:, j) - t(:, i)) / (dist + eps); % 归一化方向向量
                t(:, j) = t(:, i) + direction * ( d_min ); % 调整到边界
            end
        end
    end

    % --- 可行范围约束 ---
    % 发射天线坐标范围 [x_min, x_max] 和 [y_min, y_max]
    x_min = Ct_bounds(1, 1); x_max = Ct_bounds(1, 2);
    y_min = Ct_bounds(2, 1); y_max = Ct_bounds(2, 2);

    % 投影到发射天线范围
    t(1, :) = max(min(t(1, :), x_max), x_min); % 限制 x 坐标
    t(2, :) = max(min(t(2, :), y_max), y_min); % 限制 y 坐标

    % 接收天线坐标范围 [rx_min, rx_max] 和 [ry_min, ry_max]
    rx_min = Cr_bounds(1, 1); rx_max = Cr_bounds(1, 2);
    ry_min = Cr_bounds(2, 1); ry_max = Cr_bounds(2, 2);

    % 投影到接收天线范围
    r(1) = max(min(r(1), rx_max), rx_min); % 限制 rx
    r(2) = max(min(r(2), ry_max), ry_min); % 限制 ry

    % --- 更新位置向量 ---
    u = [t(:); r(:)];
end

% a = [1, 2 ,3; 4 ,5 ,6]
% b = a(:)
